#!/usr/bin/env python

# get vision cmd to move and feedback the TEE(translation from base to end link)
import rospy
import geometry_msgs.msg
import tf
import aubo_i5_hand_eye_cali.msg as cali_msg
import aubo_i5_hand_eye_cali.srv as cali_srv
import lcm
from mani_lcm_msgs import tee_msg

BASE_LINK = 'base_link'
LAST_LINK = 'ee_link'

success_move_count = 0
PLAN_DEFAULT_FLAG = 0
PLAN_FAIL_FLAG = 1
PLAN_SUCCESS_FLAG = 2


def send_TEE(move_success, count, status=0):
    """
    tee_msg:
        move_success: whether move done successfully
        cali_flags: [0]: whether initing 4 poses: 0 Not Initing; -100 Is Initing
                    [1]: cmd count from 0 to inf
                    [2][3]: 01, planning failed; 10, planning succeed; 00, default
    """
    global lc
    #get tee
    listener = tf.TransformListener()
    listener.waitForTransform(BASE_LINK, LAST_LINK,rospy.Time(),rospy.Duration(1.0))
    (TEE_trans,TEE_rot) = listener.lookupTransform(BASE_LINK, LAST_LINK,rospy.Time())
    #send tee
    lcm_TEE = tee_msg()
    lcm_TEE.cali_flags[0] = 0
    print "step2 count is :  ", count
    lcm_TEE.cali_flags[1] = count
    if status==PLAN_DEFAULT_FLAG:
        lcm_TEE.cali_flags[2] = 0
        lcm_TEE.cali_flags[3] = 0
    elif status==PLAN_FAIL_FLAG: #plan failed
        lcm_TEE.cali_flags[2] = 0
        lcm_TEE.cali_flags[3] = 1
    elif status==PLAN_SUCCESS_FLAG: #plan successfully
        lcm_TEE.cali_flags[2] = 1
        lcm_TEE.cali_flags[3] = 0

    if move_success:
        global success_move_count
        success_move_count += 1
        print "success move count is: ",success_move_count
        lcm_TEE.move_success = True
        lcm_TEE.translation = TEE_trans
        TEE_rot_matrix = tf.transformations.quaternion_matrix(TEE_rot)
        TEE_rotation_count = 0
        #print type(TEE_rot_matrix )
        #print TEE_rot_matrix
        for i in range(3):
            for j in range(3):
                lcm_TEE.rotation[TEE_rotation_count] = TEE_rot_matrix[i,j]
                TEE_rotation_count+=1
        #print lcm_TEE.rotation
        pass
    #else the lcm_tee is empty
    print "send TEE move_success", lcm_TEE.move_success
    print "send TEE cali_flags", lcm_TEE.cali_flags[1]
    lc.publish(TEE_LCM_CHANNEL,lcm_TEE.encode())
    pass

def command_callback(data):
    global in_motion
    if in_motion  == False:
        in_motion = True
        try:
            vertical_position = [0,0,0,0,0,0]

            # touch point
            vertical_position[0]=data.pose.position.x
            vertical_position[1]=data.pose.position.y
            vertical_position[2]=data.pose.position.z

            # current vertion orientation.w is used as rotation value in z-axis
            vertical_position[3]=data.pose.orientation.x
            vertical_position[4]=data.pose.orientation.y
            vertical_position[5]=data.pose.orientation.z

            move_in_table = rospy.ServiceProxy("move_arm_service", cali_srv.MoveArmRPYSrv)
            #raw_input("Press Enter to move")
            move_rtn = move_in_table(cali_msg.PickPoint(*vertical_position))
            print "step1 count is : ", data.pose.orientation.w #  data.pose.orientation.w is temporarily used as count
            send_TEE(move_rtn.success, data.pose.orientation.w) # send the tee information to vision
            rospy.loginfo("send TEE successfully ")

            in_motion = False
        except Exception as e:
            print e
            in_motion = False
            pass
    else:
        rospy.loginfo("Touch Interpretor received multiply commands when it's busy moving the arm.")
        # rospy.loginfo("Arm is moving, just plan this cmd")
        # plan_in_table = rospy.ServiceProxy("plan_arm_service", cali_srv.PlanArmSrv)
        # if plan_in_table(cali_msg.PickPoint(*vertical_position)):
        #     send_TEE(False, data.pose.orientation.w, PLAN_SUCCESS_FLAG) # send the tee information to vision
        # else:
        #     send_TEE(False, data.pose.orientation.w, PLAN_FAIL_FLAG) # send the tee information to vision


rospy.init_node("use_case")
rospy.Subscriber("touch_point_cmd", geometry_msgs.msg.PoseStamped, command_callback, queue_size = 1)
in_motion = False
#lcm init
TEE_LCM_CHANNEL = "TEE_LCM_CHANNEL"
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=10")


if __name__=="__main__":

    try:
        rospy.spin()
    except rospy.ROSInterruptException as e:
        print e
        pass

